package various;


import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.nxt.addon.ColorSensor;

public class ReadMatrix {
	private static int angle_x; // movimenti dx-sx
	private static int angle_y;// movimenti su-giu
	private static int cicli_int;
	private static int cicli_ex;
	private static int speed;

	public static void main(String[] args) throws Exception {

		angle_y = -30;
		angle_x = 230;
		cicli_int = 4;// colonne
		cicli_ex = 4;// righe
		speed = 75;

		int[][] matrice = new int[cicli_ex][cicli_int];

		Colori.initColori();
		ColorSensor cs = new ColorSensor(SensorPort.S1);

		calibra(cs);
		Thread.sleep(2000);

		for (int i = 0; i < cicli_ex; i++) {
			for (int j = 0; j < cicli_int; j++) {
				Motor.A.regulateSpeed(true);
				LCD.clear();
				LCD.drawString(Colori.getColore(cs.getColorNumber()), 0, 1); // legge
																				// il
																				// colore
				matrice[i][j] = cs.getColorNumber();
				Thread.sleep(1000);
				Motor.A.rotate(angle_x);
				Motor.A.stop();
			}

			riavvolgi1(angle_x, cicli_int);
			Motor.B.regulateSpeed(true);
			Motor.B.setSpeed(speed);
			Motor.B.rotate(angle_y);
			Thread.sleep(2000);
		}

		LCD.clear();
		for (int i = 0; i < cicli_ex; i++) {
			for (int j = 0; j < cicli_int; j++) {
				String w = Colori.getColoreCh(matrice[i][j]);
				LCD.drawString(w, j, i);
			}
		}
		Thread.sleep(10000);

	}

	private static void riavvolgi1(int angle, int cicli) {
		Motor.A.rotate(-angle * cicli);
	}

	private static void riavvolgi(UltrasonicSensor us) throws Exception {
		us.getDistance();
		Thread.sleep(500);
		us.getDistance();
		while (us.getDistance() >= 5) {
			Motor.A.setSpeed(300);
			Motor.A.backward();
			LCD.clear();
			LCD.drawString("cm: " + us.getDistance(), 0, 4);
		}

	}

	public static void calibra(ColorSensor cs) throws Exception {
		Motor.A.regulateSpeed(true);
		int c = 1;
		int d = 1;
		while (c == 1 && d == 1) {
			LCD.clear();
			LCD.drawString("Avvio calibrazione bianco...", 0, 2);
			c = cs.initWhiteBalance();
			LCD.drawString("Calibrazione bianco eseguita", 0, 3);
			LCD.drawInt(c, 0, 3);
			Thread.sleep(2000);

			Motor.A.rotate(angle_x);
			LCD.clear();
			LCD.drawString("Avvio calibrazione nero...", 0, 2);
			d = cs.initBlackLevel();
			LCD.clear();
			LCD.drawString("Calibrazione nero eseguita", 0, 3);
			LCD.drawInt(d, 0, 6);
			Thread.sleep(2000);
		}
		LCD.clear();
		Motor.A.regulateSpeed(true);
		Motor.A.rotate(-angle_x);
		Thread.sleep(2000);

	}
}
